Fuzzy Logic Expert Rule-based Multi-Sensor Data Fusion for Land Vehicle Attitude Estimation

In Inertial Navigation System (INS) attitude estimation dominates the accuracy of velocity and position estimation. Traditional gyro-based attitude estimation assisted with Kalman filtering is subject to unbound error growth with time especially as using lowcost Micro-Electro-Mechanical System-based (MEMS-based) sensors for land vehicle application. Thus, in recent years a low-cost INS is still limited to provide an acceptable navigation solution. This paper introduces a new fuzzy expert system to fuse multi-sensor data from MEMS accelerometers, MEMS gyroscopes and a digital compass based on their complementary characteristics related to the corresponding motion status. Field test results have shown the drift-free and smooth attitude estimation have been achieved by using our multi-sensor data fusion algorithm. The improvement of velocity and position estimation by our proposed method is significant, showing an applicable solution to land vehicle navigation using low-cost dead-reckoning sensors. INTRODUCTION The Global Positioning System (GPS) has found widely applications in land vehicle navigation, as it can provide position solutions not only cost-effective but also with longterm accuracy and availability (Parkinson and Spilker, 1996). However due to the signal fading in urban area, it requires aids from other enabling sensors. A popular solution to this problem is to integrate GPS with complementary navigation sensors such as INS, which is based on dead-reckoning methodology to obtain the position state. For land vehicle application, MEMS-based inertial sensors with low cost and small size are the affordable option. But the trade-off is the poorer performance of relatively high instrument bias, drift and noise. Based on INS mechanization, the error of velocity and position estimations will be mainly governed by the accuracy of the estimated attitude (Titterton and Weston, 1997). In traditional approach, only gyroscopes are used for attitude determination and attitude errors are compensated by Kalman filter method. But the Kalman filter is modeldependent and a priori the model parameters need to be known (Brown and Hwang, 1997). For a low-cost sensor, the behaviors of noisy and imprecise measurements are hard to model properly and the sensor biases and scale factors are dynamic and difficult to be estimated accurately. While Kalman filter is working in prediction step without measurement updates, the estimated errors are accumulating with time due to the nature of the recursive process in error state equations. Thus, a Kalman filter-based attitude estimation using low-cost gyroscopes only would result in unreliable solutions over longterm prediction. In this paper, we integrate three low-cost sensors, MEMS accelerometers, MEMS gyroscopes and a magnetometer for attitude estimation. A magnetometer with complementary characteristics to gyroscopes can provide absolute heading information relative to the magnetic north without time-accumulated error. For tilt sensing, when vehicle is static, the accelerometer measurement containing gravity field only can directly derive pitch and roll angle without time-accumulated errors. Based on the physical characteristics of each sensor, the accuracy of attitude estimation of each sensor is related to vehicle dynamics. Therefore, a fuzzy logic expert rule-based system is designed to identify the status of vehicle motion and fuse the data from these different sensor modalities. The proposed system can bound the attitude errors and reduce error growth when vehicle stop is available. Field tests using a van driven on a road are performed to examine the accuracy of vehicle attitude estimated by the proposed system. The performance improvement in velocity and position domains using the fused attitude is also discussed. ATTITUDE ESTIMATION BY MULTI-SENSORS The principle of inertial navigation is to derive the attitude, velocity and position of a moving body by measuring its dynamics based on Newton’s Law. To sense the dynamics of the vehicle, the IMU is aligned with the body frame consisting of three orthogonal axes where x is in the direction of forward motion of the vehicle and y is in the direction of transverse motion of the vehicle. In land vehicle navigation, the motion of a vehicle on the earth surface is mostly represented in the navigation frame of which the axes are aligned to the local north (n), east (e) and down (d). The transformation between the navigation frame and the body frame can be accomplished by a sequence of elementary rotations about the attitude angles. Therefore, the vehicle velocity and position in the navigation frame can be obtained when the vehicle attitude and the acceleration measured in the body frame are determined. The attitude of the vehicle is represented by three Euler angles, roll (φ ), pitch (θ ), and yaw ( ψ ), which are the rotation angles about the x, y and z axes, respectively. The changes of Euler angles, called Euler rates, are relative to the rotation rates of the body frame which can be measured by gyroscopes directly in the following manner: Bz By Bx θω tan φ cos θω tan φ sin ω φ + + = & (1) Bz By φω sin φω cos θ − = & (2) Bz By ω θ cos φ cos ω θ cos φ sin ψ + = & (3) where Bx ω , By ω , and Bz ω are angular velocity of the body frame measured by gyroscopes. The shortcoming of using gyroscopes to estimate attitude is the error accumulation due to the integration process. Even small amounts of gyro bias will result in substantial error growth of attitude without bound. Especially for low-cost sensors, attitude estimation would become unreliable since sensor errors are dynamic and much difficult to model. In contrast to gyroscopes, accelerometers can be used to directly derive vehicle pitch and roll angles while vehicle is static or moving linearly at constant speed. Under these condition vehicle pitch and roll angles can be calculated as follows. ⎟⎟ ⎠ ⎞ ⎜⎜ ⎝ ⎛ − = − g A sin φ By 1 (4) ⎟⎟ ⎠ ⎞ ⎜⎜ ⎝ ⎛ = − g A sin θ Bx 1 (5) where Bx A and By A are acceleration of the body frame measured by accelerometers and g is the local gravity field. According to equation (4) and (5), no integration is required and therefore tilt estimation error will not increase with time. The accuracy of tilt estimation mainly governed by the accelerometer bias to gravity field ratio is much better than gyro-based estimation. Thus, accelerometers can be used to bound and reset the tilt information calculated by the gyroscopes when the vehicle is not moving. (Ojeda et al., 2002) For vehicle heading determination, a magnetometer is able to provide absolute heading information relative to the magnetic north without time-accumulated errors (Caruso, 1997). But the compass measurements are still subject to the influence of nearby ferrous effects and interference. In land vehicle application, the nearby ferrous effects are mainly generated by the vehicle itself and have a weak time-variant characteristic. On the other hand, the interference is the result of magnetic disturbances from environment such as power line and it has a strong time-variant characteristic. In addition to these environmental magnetic effects, the declination angles must be determined to correct for true north. Thus, we can model the nearby ferrous effects and declination angles as the combination of bias and scale factor as follows. ψ ψ ψ n ψ̂ S b ψ̂ ψ + + + = (6) where ψ is the true heading, ψ̂ is the heading provided by magnetometer, ψ b is the sensor bias, ψ S is the scale factor, and ψ n is the noise and disturbance. After sufficient data of measurement and true value are available, the biases and scale factors can be estimated by using least squares method (Wang, 2004). It should be noticed that in land vehicle application the magnetometer is not always confined to a level plane in which the earth magnetic field stands. Thus, the tilt angles should be determined for heading corrections (Caruso, 1997). Since the tilt information is very difficult to be accurately estimated using low cost sensors when vehicle is moving, we only apply tilt compensation when vehicle is static. Thus, we can use magnetometer heading to bound and reset the heading information calculated by the gyroscopes when vehicle is not moving. Once vehicle attitude is determined, vehicle velocity and position in the navigation frame can be derived from accelerometer measurements based on vehicle dynamics model. In this paper we applied the constrained motion model proposed by Brandt and Gardner, 1998. The extra information of vehicle motion constraints can be used to reduce the navigation errors. The constrained motion model is defined as fellows. θ sin g A V Bx f − = & (7) ψ cos θ cos V x f t = & (8) ψ sin θ cos V y f t = & (9) where f V is the vehicle forward velocity. t x and t y are the vehicle coordinate in the XY plane of the earth-fixed tangent frame. Based on equation (7) to (9), the accuracy of the estimated velocity and position are mainly dominated by pitch and heading errors. Thus, in this study we only assess the accuracy of the estimated pitch and heading information. FUZZY EXPERT SYSTEM FOR MULTI-SENSOR DATA FUSION As mentioned in previous section, the performance and characteristics of each sensor are related to vehicle dynamics. Based on the knowledge of specific physical shortcomings and strengths of each sensor modality in the corresponding status of vehicle motion, vehicle attitude information can be derived from multi-sensor data. Thus, the association between raw measurements and vehicle dynamics should be investigated and identified. In this paper, we apply a fuzzy expert system for the identification of vehicle dynamics. Then, according to vehicle motion status we use the most suitable sensor to estimate vehicle attitude. In the meantime, the errors of the unused sensors are also estimated based on the statistics of observations. More specifically, we use the accelerometers and the magnetometers to derive tilt and heading information and estimate gyro drift using least squares method when vehicle is static. When vehicle is moving, we estimate vehicle attitude using compensated gyro measurements. The block diagram of our fuzzy expert system is shown in Figure 1. Magnetometer MEMS Accelerometer MEMS Gyroscope Bias/Scale Factor Compensation Initial Bias Compensation Fuzzy Expert System For Vehicle Dynamics Identification Accelerometer-Based Tilt Estimation Magnetometer-Based Heading Estimation Gyro Drift Estimation When Vehicle is Static When Vehicle is Moving Gyro-Based Attitude Estimation Initial Bias Compensation Figure 1: System Block Diagram To correctly identify vehicle dynamics (static/moving) based on low-cost sensor measurements, the identification system must have the capacity of dealing with uncertainty and imprecision due to the noisy measurements and vehicle vibration effects. Both of probability-based and fuzzy set theories can handle the uncertainty and imprecision of data. However, the failings of probability in situations where little or no a priori information is known provide an arena for the use of fuzzy expert system (Kandel, 1992). Fuzzy expert system is an expert system which incorporates fuzzy sets and/or fuzzy logic into its reasoning process and/or knowledge representation scheme. Fuzzy set theory provides a natural method for dealing with linguistic term which is a very effective knowledge representation format for imprecise and uncertain information (Kandel, 1992). Described in the following is the development of a fuzzy expert system for land vehicle dynamics identification. Shown in Figure 2 is the architecture of the fuzzy logic-based vehicle dynamics identification system. In this research, the Mamdani type fuzzy inference system, which is considered as the most commonly seen fuzzy methodology, has been used (Mamdani and Assilian, 1975). The input variables for the system are the accumulated jerk magnitude in x, y, and z direction of body frame to interpret the degree of vehicle motion. The definition of the accumulated jerk magnitude is described as follows. ( ) ∑ − = = k d k i i x x Jerk AJ (10)